
#define COLLISION_THRESHOLD 0.2

#include <iostream>
#include <cmath>
#include "CollisionWorldViewer.h"
#include "GlutStuff.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"

//#include <stdio.h> //printf debugging
#include "GLDebugDrawer.h"


static GLDebugDrawer gDebugDraw;


void CollisionWorldViewer::clientMoveAndDisplay()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

	//simple dynamics world doesn't handle fixed-time-stepping
	float ms = getDeltaTimeMicroseconds();
	
	///step the simulation
	if (m_dynamicsWorld)
	{
		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	}
		
	renderme(); 


	glFlush();

	swapBuffers();

}



void CollisionWorldViewer::displayCallback(void) {

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	
	renderme();

	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();

	glFlush();
	swapBuffers();
}

void	CollisionWorldViewer::initPhysics(){

	CollisionWorldViewer::initPhysics(Point(0,0,0));
	
}


void	CollisionWorldViewer::initPhysics(Point target)
{
	
	target.scaling(SCALING);

	setTexturing(true);
	setShadows(false);
	//setCameraUp(btVector3(btScalar(0),btScalar(30),btScalar(0)));

	//setCameraForwardAxis(0);
	setCameraDistance(5);
	CollisionWorldViewer::m_cameraTargetPosition = btVector3(btScalar(-target.getValue(Point::X)),btScalar(-target.getValue(Point::Y)),btScalar(target.getValue(Point::Z)));


	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();
	
	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
	
	m_broadphase = new btDbvtBroadphase();
	
	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;

	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	m_dynamicsWorld->setDebugDrawer(&gDebugDraw);

}

void	CollisionWorldViewer::addGround(double y)
{

	y *= SCALING;

	///Create ground
	btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(6),btScalar(0.01),btScalar(3)));
	//groundShape->initializePolyhedralFeatures();
//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
	
	m_collisionShapes.push_back(groundShape);
	 
	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,y,6.5));

	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:

	btScalar mass(0.);

	btVector3 localInertia(0,0,0);
		
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);


	//add the body to the dynamics world
	m_dynamicsWorld->addRigidBody(body);
}

void	CollisionWorldViewer::addDefaultSizedRigidBody(Point p){

	p.scaling(SCALING);

	//create a few dynamic rigidbodies
	// Re-using the same collision is better for memory usage and performance

	btBoxShape* colShape = new btBoxShape(btVector3(cubeSize, cubeSize, cubeSize));
	//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
	m_collisionShapes.push_back(colShape);

	/// Create Dynamic Objects
	btTransform startTransform;
	startTransform.setIdentity();

	btScalar	mass(0.f);

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	btVector3 localInertia(0,0,0);
		
	startTransform.setOrigin(btVector3(btScalar(-p.getValue(Point::X)),btScalar(-p.getValue(Point::Y)),btScalar(p.getValue(Point::Z))));
					
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	m_dynamicsWorld->addRigidBody(body);

}



void CollisionWorldViewer::addSphere(Point p, double r){

	p.scaling(SCALING);
	r *= SCALING;

	btSphereShape* sphere = new btSphereShape(r);
	
	m_collisionShapes.push_back(sphere);

	/// Create Dynamic Objects
	btTransform startTransform;
	startTransform.setIdentity();

	btScalar	mass(0.f);

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	btVector3 localInertia(0,0,0);
		
	startTransform.setOrigin(btVector3(btScalar(-p.getValue(Point::X)),btScalar(-p.getValue(Point::Y)),btScalar(p.getValue(Point::Z))));
					
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,sphere,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	m_dynamicsWorld->addRigidBody(body);
	


}


double CollisionWorldViewer::addHand( double xc, double yc, double zc, Point graspingPoint1, Point graspingPoint2, double fingerSize, double fingerLength, double graspWidth, double handHeight, double handLength, double teta1, double teta2, double teta3){
	
	graspingPoint1.scaling(SCALING);
	graspingPoint2.scaling(SCALING);
	
	double handX, handY, handZ;
	double hip = 5;
	
	double alpha = 1.5707;						//X
	double beta =  -teta2;
	double gamma = teta1 + teta3;			//Y
	

	/*std::cout << "\nalpha: " << alpha << "\nbeta: " << beta << "\ngamma: " << gamma << std::endl;

	std::cout << "\nxc" << xc << "\nyc" << yc << "\nzc" << zc << "\n" << std::endl;
	std::cout << "\nCx: " << centroidX << "\nCy: " << centroidY << "\nCz: " << centroidZ << std::endl;
	std::cout << "\nWx: " << wristX << "\nWy: " << wristY << "\nWz: " << wristZ << std::endl;
	std::cout << "\n\nG1x: " << graspingPoint1X << "\nG1y: " << graspingPoint1Y << "\nG1z: " << graspingPoint1Z << std::endl;
	std::cout << "\n\nG2x: " << graspingPoint2X << "\nG2y: " << graspingPoint2Y << "\nG2z: " << graspingPoint2Z << std::endl;
	*/

	/*addSphere(graspingPoint1X, graspingPoint1Y, graspingPoint1Z, 0.05);
	addSphere(graspingPoint2X, graspingPoint2Y, graspingPoint2Z, 0.05);*/

	
	//First finger
	firstFinger = addCollisionBox(	graspingPoint1.getValue(Point::X) - xc*fingerLength,
									-graspingPoint1.getValue(Point::Z) + zc*fingerLength,
									graspingPoint1.getValue(Point::Y) - yc*fingerLength,
									fingerSize, 
									fingerLength, 
									fingerSize, 
									-alpha, 
									beta, 
									gamma);

	
	secondFinger = addCollisionBox( graspingPoint2.getValue(Point::X) - xc*fingerLength,
									-graspingPoint2.getValue(Point::Z) + zc*fingerLength,
									graspingPoint2.getValue(Point::Y) - yc*fingerLength,
									fingerSize, 
									fingerLength, 
									fingerSize, 
									-alpha, 
									beta, 
									gamma);

	

	
	handX = (graspingPoint1.getValue(Point::X) + graspingPoint2.getValue(Point::X)) / 2;
	handY = (-graspingPoint1.getValue(Point::Z) - graspingPoint2.getValue(Point::Z)) / 2;
	handZ = (graspingPoint1.getValue(Point::Y) + graspingPoint2.getValue(Point::Y)) / 2;
	
	hand = addCollisionBox(	handX - xc * (fingerLength*2 + handHeight + COLLISION_THRESHOLD),
							handY + zc * (fingerLength*2 + handHeight + COLLISION_THRESHOLD),
							handZ - yc * (fingerLength*2 + handHeight + COLLISION_THRESHOLD),
							handLength, 
							handHeight, 
							graspWidth + fingerSize*2,
							-alpha,
							beta,
							gamma);
	

	

	return checkCollisions();
	
}




btRigidBody* CollisionWorldViewer::addCollisionBox(double posX, double posY, double posZ, double sizeX, double sizeY, double sizeZ, double alpha, double beta, double gamma){


	//create a few dynamic rigidbodies
	// Re-using the same collision is better for memory usage and performance

	btBoxShape* colShape = new btBoxShape(btVector3(sizeX, sizeY, sizeZ));
	//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
	m_collisionShapes.push_back(colShape);

	/// Create Dynamic Objects
	btTransform startTransform;
	startTransform.setIdentity();

	btScalar	mass(0.f);

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	btVector3 localInertia(0,0,0);
		
	startTransform.setOrigin(btVector3(btScalar(-posX),btScalar(-posY),btScalar(posZ)));
	
	btQuaternion* quaternion = new btQuaternion;
	btQuaternion* quaternion1 = new btQuaternion;
	btQuaternion* quaternion2 = new btQuaternion;
	btQuaternion* quaternion3 = new btQuaternion;
	
	/*quaternion1->setRotation(btVector3(0,0,0),90);
	quaternion2->setRotation(btVector3(0,1,0),90);
	quaternion3->setRotation(btVector3(0,0,1),90);*/

	//quaternion->setEuler(yaw,pitch,roll);
	//quaternion->setRotation(btVector3(btScalar(1),btScalar(1),btScalar(0)),1.57);

	
	quaternion1->setRotation(btVector3(btScalar(1),btScalar(0),btScalar(0)),alpha);
	quaternion2->setRotation(btVector3(btScalar(0),btScalar(0),btScalar(1)),beta);
	quaternion3->setRotation(btVector3(btScalar(0),btScalar(1),btScalar(0)),gamma);


	*quaternion = *quaternion3 * *quaternion2 * *quaternion1;
	quaternion->setEuler(alpha,beta,gamma);

	//*quaternion = *(quaternion1) * *(quaternion2) * *(quaternion3);
	
	startTransform.setRotation(*(quaternion));


	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	m_dynamicsWorld->addCollisionObject(body);

	
	return body;
}



	

void CollisionWorldViewer::removeHand(){
	
	if(firstFinger != NULL)
		m_dynamicsWorld->removeCollisionObject(firstFinger);

	if(secondFinger != NULL)
		m_dynamicsWorld->removeCollisionObject(secondFinger);

	if(hand != NULL)
		m_dynamicsWorld->removeCollisionObject(hand);

	firstFinger = NULL;
	secondFinger = NULL;
	hand = NULL;

}


void	CollisionWorldViewer::clientResetScene()
{
	exitPhysics();
	initPhysics();
}
	

int		CollisionWorldViewer::checkCollisions(){

	struct   MyContactResultCallback : public btCollisionWorld::ContactResultCallback
	{
		bool m_connected;
		btScalar m_margin;
		MyContactResultCallback() :m_connected(false),m_margin(0.05){}


		virtual   btScalar   addSingleResult(btManifoldPoint& cp,   const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
		{
			m_connected = true;
			return 1.f;
		}


	};

	MyContactResultCallback callback;
	callback.m_connected = false;

	
	m_dynamicsWorld->contactTest(firstFinger, callback);
	m_dynamicsWorld->contactTest(secondFinger, callback);
	m_dynamicsWorld->contactTest(hand, callback);
	
	if(callback.m_connected)
		return 0;

	return 1;
	

}

void	CollisionWorldViewer::exitPhysics()
{

	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear();

	delete m_dynamicsWorld;
	
	delete m_solver;
	
	delete m_broadphase;
	
	delete m_dispatcher;

	delete m_collisionConfiguration;

	
}